package com.arithmetic;

import android.util.Log;

import com.DataProcess.Robot;
import com.path.CreatePath;
import com.pudu.mydemo.nativeClass.NativeClass;

/**
 * Created by tjpu on 2016/10/12.
 */
public class InitNative {
    static String thisLogTag = "InitNative";

    public static void initNative() {
        setLocalization();
        setMotionControl();
        if(Robot.track != null && Robot.uwbStation != null && Robot.uwbStation.length > 3){
            Robot.isInitNative = true;
        }
        setOpt();
    }

    public static void setOpt() {
        if(!Robot.isInitNative)
            return;
        NativeClass.cSetK(0, Robot.K0);
        NativeClass.cSetK(1,Robot.K1);
        NativeClass.cSetK(2,Robot.K2);
        NativeClass.cSetK(3,Robot.K3);
        NativeClass.cSetK(4,Robot.K4);
        NativeClass.cSetK(5,Robot.K5);
        NativeClass.cSetForesee(Robot.foresee);
        NativeClass.cSetSpeed(Robot.setSpeed);
        NativeClass.cSetKp(Robot.P_track);
        NativeClass.cSetKd(Robot.D_track);
        MotorsControl.left.setPID(Robot.P_speed,Robot.I_speed,Robot.D_speed);
        MotorsControl.right.setPID(Robot.P_speed,Robot.I_speed,Robot.D_speed);
    }

    double[] tagInit = {-0.155, -0.024, 1.146};
    static double[] posInit = {0.0, 0.0, 0.0};
    static void setLocalization() {
        if (Robot.uwbStation == null) {
            Log.e(thisLogTag,"uwbPos null");
            return;
        }

        if (Robot.uwbStation.length < 3) {
            Log.e(thisLogTag,"uwbPos less");
            return;
        }
        NativeClass.cLocalization(Robot.uwbStation,1.18);
        NativeClass.cInitPose(posInit,Robot.UWB_TAG_POS);
    }

    static void setMotionControl() {
        if (Robot.track == null) {
            Log.e(thisLogTag,"no track");
            return;
        }

        CreatePath path2 = new CreatePath();
        double [] pathTOControl = path2.createPath(Robot.track );

        NativeClass.cMotionControl(pathTOControl);
        NativeClass.cSetSpeed(Robot.setSpeed);
    }
}
